#ifndef GPU_POISSON_CUDA_H
#define GPU_POISSON_CUDA_H

#include "GPUPoissonState.h"
#include <vector>

void knn(float* ref, int ref_width, float* query, int query_width, unsigned int k, float* dist, int* ind);
void computeDistancesTreeNodesToStates(int queryWidth, float* treeNodes, int* nodeIndices, float* states, float* distR3, float* distSO3);
void computeIntersectionStates(int queryWidth, float* treeNodes, int* nodeIndices, float* states, float* distR3, float* distSO3, 
	float* intersectionStates, int* treeNodeSampleIndices);
void computeDistanceToDest(int queryWidth, int k, float* samples, int* inIndices, float* inDistances, float* destStates, 
	float* outDistances);
void computeMinDistanceSamples(int queryWidth, int k, float* samples, int *inIndices, float* inDistances, int* outIndices, float* outDistances);
void checkCollision(int queryWidth, float* treeNodes, int* inTreeIndices, float* samples, int* inSampleIndices, int* outResult, float* colFreeStates);
void addNewStates(int queryWidth, float* samples, int* fromTreeIndices, int* toSampleIndices,
	float* treeNodes, int* nodeIndices, int* parentIndices, unsigned int* numNodes, float* colFreeStates, int* colFreeResult);

namespace ompl
{
	namespace app
	{
		class GeometrySpecification;
	}
}

namespace poisson
{

class GPUPoissonCuda
{
public:
	GPUPoissonCuda();
	virtual ~GPUPoissonCuda();

	void setup(const std::vector<float>& vBoundsMin, const std::vector<float>& vBoundsMax,
		const ompl::app::GeometrySpecification* geometrySpecificationfloat, 
		float stateValidityCheckingResolutionR3, float stateValidityCheckingResolutionSO3);
	void solve();
	void reset();
	void freeMemory();

	std::vector<GPUPoissonState>& getStates() { return states_; }

	void addStartGoalPosition(const float* startGoalPos);

protected:
	void initializeMemory();
	void uploadData(const std::vector<float>& vBoundsMin, const std::vector<float>& vBoundsMax);

	void generateMRandomStates();
	void computeNearestNodes(bool startTree);
	void computeNeighborSample(bool expandStartTree);
	void checkCollision(bool expandStartTree);

	bool resetRequired_;
	std::vector<GPUPoissonState> states_;

	float stateValidityCheckingResolutionR3_;
	float stateValidityCheckingResolutionSO3_;
};

}

#endif